#include "stdafx.h"
#define _USE_MATH_DEFINES
#include <math.h>
#include "Geo.h"

namespace Geo
{
	UtmCoord::UtmCoord(double easting, double northing, int zone, bool southHemisphere)
	{
		// TODO: validate the numbers. 
		// Q: out of bound coordinates allowed?
		this->northing = northing;
		this->easting = easting;
		this->zone = zone;
		this->southHemisphere = southHemisphere;
	}

	UtmCoord::UtmCoord(const LatLongCoord& coord, Datum datum)
	{
		// TODO: support datums other than WGS84
		// References:
		// http://en.wikipedia.org/wiki/Universal_Transverse_Mercator_coordinate_system
		// http://www.uwgb.edu/dutchs/UsefulData/UTMFormulas.htm

		// Constants for WGS84
		// Q: is the compiler going to optimize these calculations by constant propagating?
		double a = 6378137.0;		// equatorial Radius
		double f = 1/298.2572236;	// flattening
		double b = a*(1-f);			// polar radius
		double k0 = 0.9996;			// scale on central meridian

		double e = sqrt(1 - (b/a)*(b/a));	// eccentricity
		double esq = (1 - (b/a)*(b/a));		// e squared for use in expansions
		double e0sq = e*e/(1-e*e);			// e0 squared - always even powers

		// Calculate zone based on longitude
		this->zone = 1 + (int)floor((coord.longitude/3600+180)/6);
		this->southHemisphere = coord.latitude < 0;

		double phi = coord.latitude/3600 * M_PI/180; // latitude to radians
		double lambda = (fmod(coord.longitude/3600+180,6)-3)*M_PI/180; // longitude to radians

		double v = 1/sqrt(1-pow(e*sin(phi),2));
		double T = pow(tan(phi),2);
		double C = e0sq*pow(cos(phi),2);
		double A = lambda*cos(phi);
		double s = (phi*(1 - esq*(1.0/4.0 + esq*(3.0/64.0 + esq*5.0/256.0)))
			- sin(2*phi)*(esq*(3.0/8.0 + esq*(3.0/32.0 + esq*45.0/1024.0)))
			+ sin(4*phi)*(esq*esq*(15.0/256.0 + esq*45.0/1024.0))
			- sin(6*phi)*(esq*esq*esq*35.0/3072.0));
		this->easting = 500000.0 + k0*a*v*A*(1.0 + A*A*((1.0-T+C)/6.0 + A*A*(5.0 - 18.0*T + T*T)/120.0));
		this->northing = k0*a*(s + v*tan(phi)*(A*A*(1/2.0 + A*A*((5.0 - T + 9.0*C + 4*C*C)/24.0 + A*A*(61.0 - 58.0*T + T*T + 600.0*C - 330.0*e0sq)/720.0))))
			+ (this->southHemisphere ? 10000000 : 0);
	}

	LatLongCoord::LatLongCoord(double latitude, double longitude)
	{
		this->latitude	= latitude;
		this->longitude = longitude;
	}

	LatLongCoord::LatLongCoord(const UtmCoord& coord, Datum datum)
	{
		// References
		// http://www.uwgb.edu/dutchs/UsefulData/UTMFormulas.htm
		// Snyder, J. P., 1987; Map Projections - A Working Manual. U.S. Geological Survey Professional Paper 1395

		// Nothern hemisphere only!!
		double a = 6378137.0;		// equatorial Radius
		double f = 1/298.2572236;	// flattening
		double b = a*(1-f);			// polar radius
		double k0 = 0.9996;			// scale on central meridian

		double e = sqrt(1 - (b/a)*(b/a));	// eccentricity
		double esq = (1 - (b/a)*(b/a));		// e squared for use in expansions
		double e0sq = e*e/(1-e*e);			// e0 squared - always even powers

		double e1 = (1 - sqrt(1 - e*e))/(1 + sqrt(1 - e*e));			// called e1 in USGS PP 1395 also
		double M = (coord.northing-(coord.southHemisphere?10000000.0:0))/k0;	// arc length along standard meridian. 

		double mu = M/(a*(1.0 - esq*(1/4.0 + esq*(3.0/64.0 + esq*5.0/256.0))));
		double phi1 = mu + e1*(3.0/2.0 - 27.0*e1*e1/32.0)*sin(2.0*mu) + e1*e1*(21.0/16.0 -55.0*e1*e1/32.0)*sin(4.0*mu) 
			+ e1*e1*e1*(sin(6.0*mu)*151.0/96.0 + e1*sin(8*mu)*1097.0/512.0);
		double C1 = e0sq*pow(cos(phi1),2);
		double T1 = pow(tan(phi1),2);
		double N1 = a/sqrt(1-pow(e*sin(phi1),2));
		double R1 = N1*(1-e*e)/(1-pow(e*sin(phi1),2));
		double D = (coord.easting-500000.0)/(N1*k0);
		double phi2 = (D*D)*(1.0/2.0 - D*D*(5.0 + 3.0*T1 + 10.0*C1 - 4.0*C1*C1 - 9.0*e0sq)/24.0)
			+ pow(D,6)*(61.0 + 90.0*T1 + 298.0*C1 + 45.0*T1*T1 -252.0*e0sq - 3.0*C1*C1)/720.0;

		double phi = phi1 - (N1*tan(phi1)/R1)*phi2;
		double lambda = D*(1 + D*D*((-1.0 -2.0*T1 -C1)/6.0 + D*D*(5.0 - 2.0*C1 + 28.0*T1 - 3.0*C1*C1 +8.0*e0sq + 24.0*T1*T1)/120.0))/cos(phi1);

		latitude = phi*180/M_PI * 3600;
		longitude = (3 + 6*(coord.zone-1)-180+lambda*180/M_PI) * 3600;
	}

	char LatLongCoord::GetBand() const
	{
		return "CDEFGHJKLMNPQRSTUVWX"[(int) floor(this->latitude/8+10)];
	}

}